// this is the header file that tells the compiler what pins and ports, etc.
 // are available on this chip.
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <stdio.h>
 #include <stdint.h>
 #include "motorControl.h"
 #include "encoder.h"
 #include "sensors.h"
 #include "pwm.h"
 
 // define what pins the LEDs are connected to.

 //#define Warn PD6
 //#define Red_L PD7
 #define Red PB2
 #define Yellow PB1
 #define Green PB0


 #define FOSC 20000000
 #define BAUD 9600
 #define MYUBRR (((FOSC/16)/BAUD)-1)

// Some macros that make the code more readable
 //#define output_low(port,pin) port &= (uint8_t)~(1<<pin)
 //#define output_high(port,pin) port |= (uint8_t)(1<<pin)
 //#define set_input(portdir,pin) portdir &= (uint8_t)~(1<<pin)
 //#define set_output(portdir,pin) portdir |=(uint8_t)(1<<pin)

#define set(p,r) ((p) |= (bit(r)))
#define clear(p,r) ((p) &= ~(bit(r)))
#define bit(x) (0x01 << (x))
 
#define startTimer1 TCCR1B|=0x05 //1024 prescale
#define stopTimer1 TCCR1B&=~0x07

#define startTimer0 TCCR0B|=0x05 //1024 prescale
#define stopTimer0 TCCR0B&=~0x07
 
void ioinit(void);      // initializes IO
static int uart_putchar(char c, FILE *stream);
uint8_t uart_getchar(void);
static int uart_putdata(char c);
uint8_t uart_getdata(void);
static FILE mystdout = FDEV_SETUP_STREAM(uart_putchar, NULL, _FDEV_SETUP_WRITE);

 void Move(void);
 void Debug(void);
 void SendSensorData(void);
 char MyFakeData[15];

 volatile uint8_t turns;
 volatile uint8_t revs;
 volatile uint8_t TimerCount;
 char DataPacket[8];
 volatile uint16_t LOS;

 // this is just a program that 'kills time' in a calibrated method
 void delay_ms(uint8_t ms) {
   uint16_t delay_count = F_CPU / 17500;
   volatile uint16_t i;
 
   while (ms != 0) {
     for (i=0; i != delay_count; i++);
     ms--;
   }
 }

 int main(void) {
   // initialize the direction of PORTD #6 to be an output
	uint8_t count;

    ioinit(); //Setup IO pins and defaults  
	init_Motor();
	//init_pwm();
    //init_adc(); //initialize ADC
	//int_Encoders();

	//FadeData
	MyFakeData[0]='F';
	MyFakeData[1]='A';
	MyFakeData[2]='K';
	MyFakeData[3]='E';
	MyFakeData[4]='_';
	MyFakeData[5]='D';
	MyFakeData[6]='A';
	MyFakeData[7]='T';
	MyFakeData[8]='A';
	MyFakeData[9]='_';
	MyFakeData[10]='Y';
	MyFakeData[11]='E';
	MyFakeData[12]='A';
	MyFakeData[13]='!';
	MyFakeData[14]='_';

	sei();
	delay_ms(20);
	output_high(PORTB,Green);
	delay_ms(100);
	output_high(PORTB,Yellow);
	delay_ms(100);
	output_high(PORTB,Red);
	delay_ms(100);
	output_low(PORTB,Yellow);
	output_low(PORTB,Red);

	set(TCCR0A , WGM01); // enable CTC on timer 0
	set(TIMSK0 , OCIE0A);//enable timer intrupt

	set(TCCR1B , WGM12); // enable CTC on timer 1
	set(TIMSK1 , OCIE1A);//enable timer intrupt


	OCR0A=0x98;
	OCR1A=0x98;
	
	LOS=640;
	startTimer1;

  while(1){
  		output_high(PORTB,Green);
	  	count = 0;
		do{
			if(UCSR0A & (1<<RXC0)){
				DataPacket[count] = uart_getdata();
				//printf("Char %c",DataPacket[count]);
				if(DataPacket[0] == 'V'){count++;}
				else{count=0;}
			}//if
           /* if(read_sensor()){
                break;//NOT SURE WHAT TO PUT HERE, WHAT ACTION WE WANT THE ROBOT TO DO ON RETURN VALUE = 1, SHOUDLN'T IT BE SAME AS RECEIVING STOP COMMAND?
            }*/
		}while(count <8);
		output_low(PORTB,Green);
		LOS=640;
		output_low(PORTB,Red);
		startTimer1;
		Move();
//        read_sensor();
//        SendSensorData();
        Debug();
				 
	}//while(1)	

 }//main


void ioinit (void){
    set_output(DDRB, Yellow );
   	set_output(DDRB, Red);
   	set_output(DDRB, Green);

    //USART 1 Baud rate: 9600
    UBRR1H = (MYUBRR>>8);
    UBRR1L = MYUBRR;
    UCSR1B = (1<<RXEN1)|(1<<TXEN1);

    //USART 1 Baud rate: 9600
    UBRR0H = (MYUBRR>>8);
    UBRR0L = MYUBRR;
    UCSR0B = (1<<RXEN0)|(1<<TXEN0);

    
    stdout = &mystdout; //Required for printf init
}

static int uart_putchar(char c, FILE *stream){

    if (c == '\n'){
	loop_until_bit_is_set(UCSR1A, UDRE1);
    UDR1 = '\r';
	} 
    loop_until_bit_is_set(UCSR1A, UDRE1);
    UDR1 = c;
    
    return 0;
}

uint8_t uart_getchar(void){
    while(!(UCSR1A & (1<<RXC1)) );
    return(UDR1);
}

static int uart_putdata(char c){

    loop_until_bit_is_set(UCSR0A, UDRE0);
    UDR0 = c;
   
	return 0;
}

uint8_t uart_getdata(void){
    while(!(UCSR0A & (1<<RXC0)) );
    return(UDR0);
}

void Move(void){
if(DataPacket[0]=='V'){
	if ((DataPacket[2] >= 'A' && DataPacket[2] <= 'Z') && (DataPacket[3] >= 'A' && DataPacket[3] <= 'Z')){
	//forward
	forward(10);
	} else if ((DataPacket[2] >= 'a' && DataPacket[2] <= 'z') && (DataPacket[3] >= 'a' && DataPacket[3] <= 'z')){
	//revrse
	reverse(10);
	} else if ((DataPacket[2] >= 'a' && DataPacket[2] <= 'z') && (DataPacket[3] >= 'A' && DataPacket[3] <= 'Z')){
	//Left Turn
	leftTurn(10);
	} else if ((DataPacket[2] >= 'A' && DataPacket[2] <= 'Z') && (DataPacket[3] >= 'a' && DataPacket[3] <= 'z')){
	//Right Turn
	rightTurn(10);
	} else if ((DataPacket[2] == '!' && DataPacket[3] == '!')){
	lowBrake(10);
	} else if ((DataPacket[2] == '$' && DataPacket[3] == '$')){
	//do nothing is a sync packet
	} else {
	stop(10);
	}
	TimerCount=((DataPacket[1]-64)*8);
	output_high(PORTB,Yellow);
	startTimer0;
    }
}

void Debug(void){
    if(DataPacket[0]=='V'){
        if ((DataPacket[2] >= 'A' && DataPacket[2] <= 'Z') && (DataPacket[3] >= 'A' && DataPacket[3] <= 'Z')){
            //forward
            printf("Forward\n");
        } else if ((DataPacket[2] >= 'a' && DataPacket[2] <= 'z') && (DataPacket[3] >= 'a' && DataPacket[3] <= 'z')){
            //revrse
            printf("Reverse\n");
        } else if ((DataPacket[2] >= 'a' && DataPacket[2] <= 'z') && (DataPacket[3] >= 'A' && DataPacket[3] <= 'Z')){
            //Left Turn
            printf("LeftTrn\n");
        } else if ((DataPacket[2] >= 'A' && DataPacket[2] <= 'Z') && (DataPacket[3] >= 'a' && DataPacket[3] <= 'z')){
            //Right Turn
            printf("RightTn\n");
        } else if ((DataPacket[2] == '!' && DataPacket[3] == '!')){
            printf("Break\n");
        } else if ((DataPacket[2] == '$' && DataPacket[3] == '$')){
            printf("Sync\n");
        } else {
            printf("Stop!\n");
        }
    }
}

//void SendSensorData(void){
//    for(uint8_t i = 0;i<15;i++){
//        uart_putdata(sensorArray[i]);
//	}
//}

ISR(TIMER0_COMPA_vect){
TimerCount--;
if(TimerCount==0){
		stopTimer0;
		output_low(PORTB,Yellow);
		stop(10);
	}
}

ISR(TIMER1_COMPA_vect){
LOS--;
if(LOS==0){
		LOS=640;
		output_high(PORTB,Red);
		printf("LOS!\n");
 }
}

ISR(PCINT2_vect){
	turns++;
	if(turns >=140){
		highBrake(10);
		//printf("Stop!, Turns = %d, Revs = %d\n", turns, revs);
		turns = 0;
		revs = 0;
	}

	}
